// ----------------
// Project:
// ESA Elevator
// ----------------
//
// ----------------
// Group: 163
// 2358569: Pascal Klink
// 2649456: Lukas Kreh 
// ----------------
//
// Description:
// ----------------
// elevator motor-interface (door and elevator)
//
// Version History:
// ----------------
// 140116: added µ to corresponding comment-lines
// 140116: deleted parameter MOTOR_STEP_BITS2

`timescale 1ns / 1ns

module motor_if

         #(parameter INITIAL_POSITION  =       0,      // inital position is at floor 0
                     MOTOR_STEP        =      50,      // [m] (a single motor step is 50µm = 0,05mm)
                     CONTROL_STEP      =      10,      // [mm] (a control step is 10mm = 1cm)
                     DISTANCE_BITS     =      14,      // max distance 12m = 12.000 mm
                     CONTROL_STEP_BITS =      11,      // 12m <> 1.200 control steps
                     MOTOR_STEP_BITS   =      18,      // max distance 12m = 240.000 * 50µm
                     MAX_POSITION_R    =    1200,      // [control steps] (1.200 control steps = 240.000 motor steps = 12 m)
                     MAX_POSITION_L    =       0,      // [control steps] (0 control steps = 0 motor steps = 0 m)
                     DELAY_COUNT_BITS  =      17,      // 17 bits required to count up to C_0 = 100.000
                     C_0               =  100000,      // initial counter value <> 100 Hz @ 10 MHz clock
                     C_MIN             =     250)      // minimum counter value <> 40 kHz @ 10 MHz clock

         (input  wire                      CLK,
          input  wire                      RESET,

          input  wire [DISTANCE_BITS-1 :0] DISTANCE,        // levels [mm] / door_width [mm]
          input  wire                      ROTATE_LEFT,     // open or down
          input  wire                      ROTATE_RIGHT,    // close or up
                                            
          output reg                       CTRL_STEP_DONE,  // 10mm distance is a step
          output reg                       DONE,            // motor request executed
          output wire                      A,               // motor coil a output
          output wire                      B,               // motor coil a output
          output wire                      C,               // motor coil a output
          output wire                      D);              // motor coil a output

/* =============================INSERT CODE HERE======================================*/ 

	
	//Local parameter holding the number of motor steps which have to be to do a control step 
	localparam MOTOR_STEPS_PER_CTRL = 		CONTROL_STEP * 1000 / MOTOR_STEP;
	//Local parameter holding the number of bits needed, to display the number of motor steps per control step
	localparam MOTOR_STEPS_PER_CTRL_BITS =	min_1(ceil_log2(MOTOR_STEPS_PER_CTRL));
	//Local parameters for clarifying the meaning of the state values
	localparam waiting = 0;
	localparam accelerating = 1;
	localparam driving = 2;
	localparam decelerating = 3;
	
	/****************************************
	* Submodule registers and instantiation *
	****************************************/
	
	//Registers for telling the module to calculate the next or previous root
	reg												next_root, previous_root;
	//Module signal saying the that root calculation is complete
	wire												root_done;
	//Signal used to reset the root module (not only connected to global reset, see combinational block)
	wire												root_reset;
	//The root calculated by the module
	wire [31:0]										root_result;
	
	sqrt root(CLK, root_reset, next_root, previous_root, root_done, root_result);
	
	/*********
	* Module *
	*********/
	 
	//The calculation of the next number of CLK cycles to be waited (see logic block)
	wire [(DELAY_COUNT_BITS + 32)- 1:0]		delay_calculation;
	/*
	* Registers storing the number of CLK cycles to wait before doing the next motor step
	* (current_delay) and the number of CLK cycles which have been waited since the last 
	* motor step
	*/
	reg [DELAY_COUNT_BITS - 1:0]				current_delay, delay_count;
	//Register for storing the state of the module
	reg [1:0]										state;
	/*
	* Registers for storing the motor steps to be done to complete the command and the motor
	* steps done withing the current command (current_motor_steps_um)
	*/
	reg [(DISTANCE_BITS + 10) - 1:0]			motor_steps_command_um, current_motor_steps_um;
	/*
	* Register used to save the maximum number of motor steps available for acceleration. If
	* the motor reaches the maximum speed before the maximum number of motor steps were done,
	* the motor steps needed to get to the maximum speed are stored in motor_steps_decel_um
	*/
	reg [(DISTANCE_BITS + 9) - 1:0]			motor_steps_accel_um, motor_steps_decel_um;
	/*
	* Register for generating the CONTROL_STEP_DONE singals the actual position tracking
	* is done with total_control_steps since this module works with motor_steps
	*/
	reg [MOTOR_STEP_BITS - 1:0]				total_motor_steps;
	//Register for storing the number of motor steps which have been done since the last control step
	reg [MOTOR_STEPS_PER_CTRL_BITS - 1:0]	motor_steps_in_ctrl;
	/*
	* Register used to store the previously calculated root, so that the next wait interval
	* can be calculated (see logic block) 
	*/
	reg [31:0]										current_root;
	/*
	* Register for storing the position of the module in motor steps (in withtin [MIN_POSITION,
	* MAX_POSITION] * 1000 - * 1000 to get from mm to um)
	*/
	reg [CONTROL_STEP_BITS - 1:0]				total_control_steps;
	//Resister for storing the direction of the command being processed (going up or down)
	reg 												direction;
	//The distance to be driven in the new command converted to um (and more - see logic block)
	reg [(DISTANCE_BITS + 10) - 1:0]			distance_in_um;
	//Helpers used to be able to set the motor signals within an always block
	reg												a_tmp, b_tmp, c_tmp, d_tmp;
	//Register to manipulate the next delay calculation (see logic block)
	reg												calculation_mode;
	/*
	* Register used to be able to correct the root by one step downwards (setting the previous_root)
	* signal another time after the first calculation has finished. This is needed in some cases to
	* be able to end with the same wait interval (C_0) the motor has started the command 
	*/
	reg [1:0]										root_correct;
	//Register used to be able to reset the root module form within the motor module
	reg												internal_reset;
	
	/*******************
	* Sequential Logic *
	*******************/
	
	always @ (posedge CLK) begin
		//The following if clauses ensure that the output signals are only high for one clock cycle
		if(next_root) next_root 				<= 0;
		if(previous_root) previous_root 		<= 0;
		if(CTRL_STEP_DONE) CTRL_STEP_DONE	<= 0;
		if(internal_reset) internal_reset	<= 0;
		if(RESET) begin
			next_root 					<= 0;
			previous_root 				<= 0;
			
			current_delay 				<= C_0;
			delay_count 				<= 0;
			
			state 						<= 0;
			
			motor_steps_command_um 	<= 0;
			motor_steps_accel_um 	<= 0;
			motor_steps_decel_um 	<= 0;
			total_motor_steps 		<= INITIAL_POSITION * MOTOR_STEPS_PER_CTRL;
			motor_steps_in_ctrl		<= 0;
			
			current_root 				<= 0;
			
			total_control_steps 		<= INITIAL_POSITION;
			direction 					<= 0;
			
			calculation_mode			<= 0;
			root_correct				<= 0;
			
			CTRL_STEP_DONE				<= 0;
			
			internal_reset				<= 0;
		end
		/*
		* Check if the motor should be halted, if it is so, the given command is ommitted and
		* the root module is resetted (here is a usage of internal_reset)
		*/
		else if(ROTATE_LEFT & ROTATE_RIGHT) begin
			state							<= 0;
			internal_reset				<= 1;
		end
		else begin
			/*
			* In the waiting state, the module waits for a new command. If it recieves one
			* (ROTATE_RIGHT | ROTATE_LEFT), the wait interval (current_delay) is set to C_0
			* the delay count is set to 0 (since 0 clock cycles have been waited). The given
			* distance of the command (distance_in_um) is assigned to the motor_steps_command_um
			* and motor_steps_accel_um is initialised with the half number of the motor steps to be done
			* in the command. (For a uneven number of motor_steps, the resulting inaccuracy is solved
			* in the accelerating state)
			* Furthermore it is checked if the given command only takes on motor step to be completed, then
			* the accelerating state is skipped.
			*/
			if(state == waiting) begin
				if(ROTATE_RIGHT | ROTATE_LEFT) begin
					current_root				<= 0;
					
					current_delay 				<= C_0;
					delay_count					<= 0;
					
					motor_steps_command_um	<= distance_in_um;
					motor_steps_accel_um 	<= distance_in_um >> 1;
					motor_steps_decel_um 	<= 0;
					current_motor_steps_um	<= 0;
					
					direction					<= ROTATE_LEFT ? 1 : 0;
					
					if((distance_in_um >> 1) >= MOTOR_STEP) begin
						state 					<= 1;
						current_root			<= root_result;
						next_root				<= 1;
					end
					else begin
						state						<= 3;
					end
					calculation_mode			<= 0;
					root_correct				<= 0;
				end
			end
			/*
			* The following steps are always done regardless of whether the module is in state accelerating, decelerating
			* or driving
			*/
			else begin
				/*
				* Check if the distance to be driven in the command should be updated. If it is so, only take the update, if
				* the motor has enough time to decelerate with the new distance. A special case is if the new distance matches
				* exactly the number of motor steps needed to decelerate. Than the module instantly switches to state 3, changes
				* the calculation mode and corrects the root (since we precalculate the root used for the next delay, we have to correct this wrong
				* calculation) - this is one usage of root_correct. The other one can be found when dealing with uneven number of motor steps
				*/
				if((ROTATE_LEFT & direction) | (ROTATE_RIGHT & !direction)) begin
					if(state == 2 & distance_in_um >= motor_steps_decel_um | state == 1 & distance_in_um >= current_motor_steps_um) begin
						motor_steps_command_um 	<= current_motor_steps_um + distance_in_um;
						motor_steps_accel_um 	<= (current_motor_steps_um + distance_in_um) >> 1; 
						if(state == 2 & distance_in_um == motor_steps_decel_um | state == 1 & distance_in_um == current_motor_steps_um) begin
							state 					<= 3;
							root_correct			<= 1;
							calculation_mode		<= 1;
						end
					end
				end
				/*
				* If root_correct has been set, this logic block waits for the current root calculation to finish
				* and then invokes another calculation of the previous_root. Note that because of the root_correction
				* the minimum wait interval should not be smaller than 38 cycles (since 32 cycles are needed to calculate
				* the two roots and 6 cycles are lost by pipelining effects of the registers)
				*/
				if(root_correct[0] & root_done) begin
					if(root_correct == 3) begin
						root_correct						<= 0;
						current_root 						<= root_result;
						previous_root						<= 1;
					end
					else begin
						root_correct						<= 3;
					end
				end
				/*
				* Check if enough cycles have been waited and a motor step should be done. Since we count starting from
				* zero, we only have to count to current_delay - 1 (same for motor_steps_in_ctrl). If it is so, the new delay
				* is assigned to current_delay, the delay_count is set to 0 and the motor steps (total and in command) are updated.
				* If enough motor steps have been done, a the control step counter will be updated and CONTROL_STEP_DONE is set to 
				* high for one clock cycle. If not enough cycles have been waited yet, we increase the step counter (see far down there ;) )
				*/
				if(delay_count >= current_delay - 1) begin
					delay_count 							<= 0;
					current_delay 							<= delay_calculation[(DELAY_COUNT_BITS - 1) + 16:16] < C_MIN ? C_MIN : delay_calculation[(DELAY_COUNT_BITS - 1) + 16:16];
					current_motor_steps_um				<= current_motor_steps_um + MOTOR_STEP;
					total_motor_steps						<= direction ? total_motor_steps - 1 : total_motor_steps + 1;
					if(motor_steps_in_ctrl == MOTOR_STEPS_PER_CTRL - 1) begin
						motor_steps_in_ctrl				<= 0;
						CTRL_STEP_DONE						<= 1;
						total_control_steps				<= direction ? total_control_steps - 1 : total_control_steps + 1;
					end
					else begin
						motor_steps_in_ctrl				<= motor_steps_in_ctrl + 1;
					end
					/*
					* From here the handling of the events differ from state to state
					*/
					case(state)
						/*
						* When accelerating we decrease the next wait interval by calculating the next root whenever a motor step is done.
						* The state transitions are a bit more complicated here, since there are three different cases:
						* 	1.	When the motor reaches the maximum speed, it switches the calculation mode (since we are now beginning to calculate
						* 		previous roots), invokes the calculation of the next previous root and switches to state driving.
						*	2.	When we have and even number of motor steps, we have to start with the calculation of the previous roots one cycle
						*		before doing the last motor step of the acceleration state, since the value of the delay calculation won't change for
						* 		the first calculation of the previous root. (Little explanation: Because we also switch the calcultion mode with calculating
						*		the previous root we then calculate (current_root)n+1 - (root_result)n instead of (root_result)n+1 - (current_root)n )
						*	3. If we have to deal with an uneven number of motor steps, we even have to correct the root down one calculation step, to end
						*		up with C_0 as the last waiting interval.
						*/
						accelerating: begin
							if(delay_calculation[(DELAY_COUNT_BITS - 1) + 16:16] <= C_MIN) begin
								current_root 				<= root_result;
								previous_root 				<= 1;
								calculation_mode			<= 1;
								motor_steps_decel_um 	<= current_motor_steps_um + 2 * MOTOR_STEP;
								state 						<= 2;
							end
							else if(current_motor_steps_um + 2 * MOTOR_STEP >= motor_steps_accel_um & current_motor_steps_um + MOTOR_STEP < motor_steps_accel_um) begin
								current_root 				<= root_result;
								previous_root 				<= 1;
								calculation_mode			<= 1;
								root_correct				<= (current_motor_steps_um + 2 * MOTOR_STEP) > motor_steps_accel_um;
							end
							else if(current_motor_steps_um + MOTOR_STEP >= motor_steps_accel_um) begin
								current_root 				<= root_result;
								previous_root 				<= 1;
								calculation_mode			<= 1;
								state 						<= 3;
							end
							else begin
								current_root 				<= root_result;
								next_root 					<= 1;
							end
						end
						/*
						* When driving at maximum speed, we are just driving with the minimum wait interval, until we have to
						* decrease the cabin speed again (motor_steps_command_um + MOTOR_STEP) == motor_steps_decel_um. Then we
						* invoke the calculation of the next previous root (since we precalculated the next delay, so that it's
						* instantly ready when doing the next motor step)
						*/
						driving: begin
							if(motor_steps_command_um - (current_motor_steps_um + MOTOR_STEP) == motor_steps_decel_um) begin //done
								current_root 				<= root_result;
								previous_root 				<= 1;
								state 						<= 3;
							end
						end
						/*
						* When decelerating we calculate the next previous root when doing a motor step. When enough motor steps
						* have been done to complete the command, the module switches to state "waiting" and resets the root
						* module (just to be shure that everything is up and ready for the next calculation)
						*/
						decelerating: begin
							if(current_motor_steps_um + MOTOR_STEP == motor_steps_command_um) begin
								state 						<= 0;
								internal_reset				<= 1;
								current_root				<= 0;
							end
							else begin
								current_root 				<= root_result;
								previous_root				<= 1;
							end
						end
						default: state 					<= state;
					endcase
				end
				else begin
					delay_count 							<= delay_count + 1;
				end
			end
		end
	end
	
	/**********************
	* Combinational Logic *
	**********************/
	
	/*
	* Calculate the next delay: 
	* 	If motor is accelerating (calculation_mode == 0) then root_result is bigger than current_root, so 
	*	root_result - current_root has to be multiplied with C_0.
	* 	If motor is decelerating, it's vice versa, so we have to calculate current_root - root_result
	*/
	assign delay_calculation = C_0 * (calculation_mode == 0 ? root_result - current_root : current_root - root_result);
	
	/*
	* Distance conversion:
	*	This logic block converts the DISTANCE (given in mm) to um and furthermore checks if the motor would
	* 	exceed it's upper/lower boundary by completing the command. If this is the case, the logic block will
	* 	only convert the remaining distance to the upper/lower boundary into um
	*/
	always @ (*) begin
		if(ROTATE_LEFT)begin
			if(total_control_steps * CONTROL_STEP < MAX_POSITION_L * CONTROL_STEP + DISTANCE) begin
				distance_in_um = total_control_steps * CONTROL_STEP * 1000;  // done					
			end
			else begin
				distance_in_um = (DISTANCE * 1000); //done
			end
		end
		else begin
			if(total_control_steps * CONTROL_STEP + DISTANCE > MAX_POSITION_R * CONTROL_STEP) begin
				distance_in_um = (MAX_POSITION_R - total_control_steps) * CONTROL_STEP * 1000; //done
			end
			else begin
				distance_in_um = (DISTANCE * 1000); //done
			end
		end
	end
	
	//Generate signals for the step motor according to the motor stop to be done
	always @ (*) begin
		case (total_motor_steps[1:0])
			0: {a_tmp, b_tmp, c_tmp, d_tmp} <= 4'b1001; 
			1:	{a_tmp, b_tmp, c_tmp, d_tmp} <= 4'b1100;
			2: {a_tmp, b_tmp, c_tmp, d_tmp} <= 4'b0110;
			3: {a_tmp, b_tmp, c_tmp, d_tmp} <= 4'b0011;
		endcase
	end
	
	//Assign helper values to the real output
	assign A = a_tmp;
	assign B = b_tmp;
	assign C = c_tmp;
	assign D = d_tmp;
	
	//Set DONE to high, when motor is waiting for a command
	always @ (*) begin
		DONE = (state == 0);
	end
	
	/*
	* Reset root module not only on global reset, but also on internal_reset (when motor is halted or
	* has completed the command)
	*/
	assign root_reset = RESET | internal_reset;
	
  /************************************************************
	* Parameter Functions (taken from the provided testbenches)*
	***********************************************************/
	
  //log2(value)
  function integer ceil_log2;
    input [31:0] value;
    for (ceil_log2=0; value>0; ceil_log2=ceil_log2+1)
      value = value>>1;
  endfunction
  
  // min(1, value)
  function integer min_1;
    input [31:0] value;
    if (value == 0)
      min_1 = 1;
    else
      min_1 = value;
  endfunction 
/* ====================================================================================*/
   
endmodule 
